#include "Mavlinks.h"
#ifdef WIN32
#include <WinSock2.h>
#pragma comment(lib, "ws2_32.lib")
#else
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#endif
#include <thread>

CMavlinks::CMavlinks()
{

}

void CMavlinks::Add(unsigned char sysid, unsigned short port, unsigned short peerPort, std::string ip)
{
    struct sockaddr_in addr;
    addr.sin_addr.s_addr = inet_addr(ip.c_str());
    MavlinkInfo &mli = mapMavlink[sysid];
    mli.seq = 0;
    mli.port = port;
    mli.peerPort = peerPort;
    mli.ip = addr.sin_addr.s_addr;
    mli.mavlink.get().set("sysid", sysid);
}

CMavlink &CMavlinks::Get(unsigned char sysid )
{
    std::map<unsigned char, MavlinkInfo>::iterator it = mapMavlink.find(sysid);
    if (it != mapMavlink.end())
    {
        return it->second.mavlink;
    }
    CMavlink *p = NULL;
    return *p;
}

void CMavlinks::SendHeartBeat()
{
    static int sockfd = 0;
    if(sockfd == 0)
    {
        sockfd = socket(PF_INET, SOCK_DGRAM, 0);
        new std::thread([this]()
        {
            for(auto &a : mapMavlink)
            {
                MavlinkInfo &mli = a.second;
                mli.mavlink.get().set("seq", mli.seq++);
                std::vector<unsigned char> d = mli.mavlink.encode(0);
                int len;
                struct sockaddr_in addr;
                memset(&addr, 0, sizeof(addr)); // 数据初始化--清零
                addr.sin_family = AF_INET;      // 设置为IP通信
                addr.sin_addr.s_addr = mli.ip;  // 服务器IP地址--允许连接到所有本地地址上
                addr.sin_port = htons(mli.peerPort);
                if ((len = sendto(sockfd, d.data(), d.size(), 0, (struct sockaddr *)&addr, sizeof(struct sockaddr))) < 0)
                {
                    perror("sendto");
                }
            }
            std::this_thread::sleep_for(std::chrono::seconds(1));
        });
    }
}